import math
import numpy as np
import base64

base32Arr = ['A', 'B', 'C', 'D', 'E', 'F', 'G', 'H', 'I',
             'J', 'K', 'L', 'M', 'N', 'O', 'P', 'Q', 'R', 'S', 'T', 'U', 'V', 'W', 'X', 'Y', 'Z', '2', '3', '4', '5',
             '6', '7']


def formatH(h):
    array = [512, 256, 128, 64, 32, 16, 8, 4, 2, 1]
    arr = []
    tmp_array = []
    result_array = []
    flag = 0
    for i in range(0, 10, 1):
        tmp = 0 if h < flag else 1
        flag = (flag - array[i] / 2) if h < flag else (flag + array[i] / 2)
        arr.append(tmp)
    for i in range(0, 2, 1):
        tmp = "%d%d" % (
            arr[5 * i + 0], arr[5 * i + 1])

        tmp_array.append(tmp)
        result_array.append(base32Arr[int(tmp, 2)])
    return ''.join(result_array)


def formatCoor(x, y):
    array = [262144, 131072, 65536, 32768, 16384, 8192, 4096, 2048, 1024, 512, 256,
             128, 64, 32, 16, 8, 4, 2, 1, 0.5, 0.25, 0.125, 0.0625, 0.03125, 0.015625]
    arr = []
    tmp_array = []
    result_array = []
    flag_x = 0
    flag_y = 0
    for i in range(0, 25, 1):
        x_tmp = 0 if x < flag_x else 1
        y_tmp = 0 if y < flag_y else 1
        flag_x = (flag_x - array[i] / 2) if x < flag_x else (flag_x + array[i] / 2)
        flag_y = (flag_y - array[i] / 2) if y < flag_y else (flag_y + array[i] / 2)
        arr.append(x_tmp)
        arr.append(y_tmp)

    for i in range(0, 10, 1):
        tmp = "%d%d%d%d%d" % (
            arr[5 * i + 0], arr[5 * i + 1], arr[5 * i + 2], arr[5 * i + 3], arr[5 * i + 4])

        tmp_array.append(tmp)
        result_array.append(base32Arr[int(tmp, 2)])
    return ''.join(result_array)


def format3Bit(val):
    if (len(val) != 3) or is_number(val) == False:
        return 'FF'
    tmp = bin(int(val))[2:].zfill(10)
    return "%s%s" % (base32Arr[int(tmp[0:5], 2)], base32Arr[int(tmp[5:10], 2)])


def is_number(s):
    try:
        float(s)
        return True
    except ValueError:
        pass

    try:
        import unicodedata
        unicodedata.numeric(s)
        return True
    except (TypeError, ValueError):
        pass

    return False


def calculate_distance(point1, point2):
    # 转换为 NumPy 数组
    point1 = np.array(point1)
    point2 = np.array(point2)

    # 计算坐标差的平方
    diff = point2 - point1
    squared_diff = np.square(diff)

    # 求和并取平方根
    distance = np.sqrt(np.sum(squared_diff))

    return distance


def calculate_point_on_vector(direction_vector, point, distance):
    # direction_vector = np.array([x2 - x1, y2 - y1, z2 - z1])  # 方向向量
    length = np.linalg.norm(direction_vector)  # 方向向量的长度
    scaled_vector = (distance / length) * direction_vector  # 缩放后的方向向量
    return np.array([round(point[0] - scaled_vector[0], 3), round(point[1] - scaled_vector[1], 3),
                     round(point[2] - scaled_vector[2], 3)])  # 计算目标点坐标


# vector 为要旋转的向量
# axis 旋转轴向量
# angle 旋转角度（弧度）
def rotate_vector_around_axis(vector, axis, angle):
    # 归一化向量
    vector = vector / np.linalg.norm(vector)
    axis = axis / np.linalg.norm(axis)

    # 计算旋转四元数
    q = quaternion_from_axis_angle(axis, angle)

    # 执行四元数旋转
    rotated_vector = quaternion_rotate_vector(q, vector)

    return rotated_vector


def quaternion_from_axis_angle(axis, angle):
    # 计算旋转四元数
    angle /= 2
    axis = axis / np.linalg.norm(axis)
    q = np.array([np.cos(angle), axis[0] * np.sin(angle), axis[1] * np.sin(angle), axis[2] * np.sin(angle)])

    return q


def quaternion_rotate_vector(q, v):
    # 执行四元数旋转
    q_conj = np.array([q[0], -q[1], -q[2], -q[3]])
    rotated_vector = quaternion_multiply(quaternion_multiply(q, np.concatenate(([0], v))), q_conj)[1:]

    return rotated_vector


def quaternion_multiply(q1, q2):
    # 四元数乘法
    w1, x1, y1, z1 = q1
    w2, x2, y2, z2 = q2
    w = w1 * w2 - x1 * x2 - y1 * y2 - z1 * z2
    x = w1 * x2 + x1 * w2 + y1 * z2 - z1 * y2
    y = w1 * y2 - x1 * z2 + y1 * w2 + z1 * x2
    z = w1 * z2 + x1 * y2 - y1 * x2 + z1 * w2

    return np.array([w, x, y, z])


# 这里的参数center，指面的中心点，也就是管线的中心点
def calculate_vertex(normal, center, pipe_shape, pipe_width, pipe_height, num_interpolation_points):
    distance = 0
    angle_array = []
    if pipe_shape == 1:  # 圆管
        # num_interpolation_points = 12
        distance = pipe_height / 2
        # angle_array = [0, np.pi / 6, np.pi / 3, np.pi / 2, 2 * np.pi / 3, 5 * np.pi / 6, np.pi, 7 * np.pi / 6,
        #                4 * np.pi / 3, 3 * np.pi / 2, 5 * np.pi / 3, 11 * np.pi / 6]
        for i in range(num_interpolation_points):
            angle_array.append(2 * i * np.pi / num_interpolation_points)
    else:  # 方管
        distance = math.sqrt(math.pow(pipe_width / 2, 2) + math.pow(pipe_height / 2, 2))
        angle_tmp = math.atan(pipe_width / pipe_height)
        angle_array = [angle_tmp, np.pi - angle_tmp, np.pi + angle_tmp, 2 * np.pi - angle_tmp]

    # 旋转轴为 normal
    # 首先计算要旋转的向量
    v1 = np.cross(normal, np.array([0, 0, 1]))
    vector_rotate = np.cross(normal, v1)

    result_array = []
    for angle in angle_array:
        vector_tmp = rotate_vector_around_axis(vector_rotate, normal, angle)
        u_tmp = vector_tmp / np.linalg.norm(vector_tmp)
        pt = center + u_tmp * distance
        result_array.append((round(pt[0], 3), round(pt[1], 3), round(pt[2], 3)))

    return result_array


def divide_arc(center, start_point, end_point, n):
    v1 = start_point - center
    v2 = end_point - center

    u1 = v1 / np.linalg.norm(v1)
    u2 = v2 / np.linalg.norm(v2)

    theta = np.arccos(np.dot(u1, u2))

    rotation_axis = np.cross(u1, u2)
    rotation_axis /= np.linalg.norm(rotation_axis)

    points = []
    for i in range(n):
        angle = i * theta / (n - 1)
        rotated_point = rotate_around_axis(start_point, center, rotation_axis, angle)
        points.append(rotated_point.tolist())

    return points


# def rotation_matrix_from_axis_angle(axis, angle):
#     axis = np.asarray(axis)
#     axis /= np.linalg.norm(axis)
#     a = np.cos(angle / 2.0)
#     b, c, d = -axis * np.sin(angle / 2.0)
#     rotation_matrix = np.array([[a * a + b * b - c * c - d * d, 2 * (b * c - a * d), 2 * (b * d + a * c)],
#                                 [2 * (b * c + a * d), a * a + c * c - b * b - d * d, 2 * (c * d - a * b)],
#                                 [2 * (b * d - a * c), 2 * (c * d + a * b), a * a + d * d - b * b - c * c]])
#     return rotation_matrix


def rotate_around_axis(point, axis_origin, axis_direction, angle):
    axis_origin = np.asarray(axis_origin)
    axis_direction = np.asarray(axis_direction)
    point = np.asarray(point)

    v = point - axis_origin
    rotated_v = v * np.cos(angle) + np.cross(axis_direction, v) * np.sin(angle) + axis_direction * np.dot(
        axis_direction, v) * (1 - np.cos(angle))
    rotated_point = rotated_v + axis_origin

    return rotated_point


def find_intersection(line1_point, line1_direction, line2_point, line2_direction):
    A = np.vstack((line1_direction, -line2_direction)).T
    b = line2_point - line1_point

    try:
        t, s = np.linalg.lstsq(A, b, rcond=None)[0]
    except np.linalg.LinAlgError:
        return None

    intersection_point = line1_point + t * line1_direction
    return np.array([round(intersection_point[0], 3), round(intersection_point[1], 3), round(intersection_point[2], 3)])


# 在直线1，直线2的交点处计算相关参数
def calculate_pipe_params(start_point_line1, end_point_line1, start_point_line2, end_point_line2, turning_radius,
                          num_segments_on_arc, num_segments_per_circle_section, pipe_shape, pipe_width_line1,
                          pipe_height_line1, pipe_width_line2,
                          pipe_height_line2):
    vector_line1 = end_point_line1 - start_point_line1
    vector_line2 = start_point_line2 - end_point_line2
    tmp_end_point_line1 = calculate_point_on_vector(vector_line1, end_point_line1, turning_radius)
    tmp_start_point_line2 = calculate_point_on_vector(vector_line2, start_point_line2, turning_radius)
    plane_normal = np.cross(vector_line1, vector_line2)
    vector_line1_vertical = np.cross(plane_normal, vector_line1)
    vector_line2_vertical = np.cross(plane_normal, vector_line2)
    circle_center = find_intersection(tmp_end_point_line1, vector_line1_vertical, tmp_start_point_line2,
                                      vector_line2_vertical)
    arc_segments = divide_arc(circle_center, tmp_end_point_line1, tmp_start_point_line2, num_segments_on_arc)
    elbow_points_vertices = []

    for i in range(num_segments_on_arc):
        segment = arc_segments[i]
        tmp_pipe_width = 0
        tmp_pipe_height = 0
        if pipe_width_line1 == pipe_width_line2 and pipe_height_line1 == pipe_height_line2:
            tmp_pipe_width = pipe_width_line1
            tmp_pipe_height = pipe_height_line1
        else:
            tmp_pipe_width = pipe_width_line1 + i * (pipe_width_line2 - pipe_width_line1) / num_segments_on_arc
            tmp_pipe_height = pipe_height_line1 + i * (pipe_height_line2 - pipe_height_line1) / num_segments_on_arc
        tmp_vector_segment = np.cross(plane_normal, circle_center - segment)
        elbow_point_vertices = calculate_vertex(tmp_vector_segment, segment, pipe_shape, tmp_pipe_width,
                                                tmp_pipe_height, num_segments_per_circle_section)
        elbow_points_vertices.append(elbow_point_vertices)

    return elbow_points_vertices


def calculate_nt(num_segments_per_circle, radius, join_length, pt_o, *pt_x):
    pt_start = pt_o + np.asarray([0, 0, radius])
    normal_start = np.asarray([0, 1, 0])
    vertices_array = []
    vertices = []
    for i in range(num_segments_per_circle + 1):
        rotate_angle1 = (2 * i * np.pi) / num_segments_per_circle
        rotate_normal = rotate_vector_around_axis(normal_start, np.asarray([0, 0, 1]), rotate_angle1)
        for j in range(num_segments_per_circle + 1):
            rotate_angle2 = (j * np.pi) / num_segments_per_circle
            tmp_pt = rotate_around_axis(pt_start, pt_o, rotate_normal, rotate_angle2)
            vertices.append(tmp_pt)
    vertices_array.append(vertices)
    for i in range(len(pt_x)):
        tmp_start_vertices = []
        tmp_end_vertices = []
        pt_a = pt_x[i]
        v_oa = pt_a - pt_o
        pt_a = calculate_point_on_vector(-v_oa,pt_o,join_length)
        tmp_v = np.cross(pt_a, v_oa)
        for j in range(num_segments_per_circle + 1):
            rotate_angle = (2 * j * np.pi) / num_segments_per_circle
            rotate_v = rotate_vector_around_axis(tmp_v, v_oa, rotate_angle)
            pt_start = calculate_point_on_vector(rotate_v, pt_a, radius)
            pt_end = calculate_point_on_vector(rotate_v, pt_o, radius)
            tmp_start_vertices.append(pt_start)
            tmp_end_vertices.append(pt_end)
        vertices_array.append(tmp_start_vertices)
        vertices_array.append(tmp_end_vertices)
    return vertices_array
